机器人电流拖动功能开发

[TOC]

机器人运行在电流环时，才能使用拖动相关算法

1 设置相关参数(非实时)

1. 创建算法库实例

   /**
    * @brief 创建机器人算法库实例
    * @param modelName：机器人对应的 URDF 描述文件
    * @param path: 机器人资源文件的路径
    * @return 算法库实例指针,如果失败, 返回 nullptr
    * ARAL算法库文件可以通过 dlopen 在运行时打开, 通过调用 CreateRLIntfacePtr 函数得到基类接口指针
    */
   ARAL::interface::ARALIntfacePtr CreateARALIntfacePtrFromFile(const char* model_name, const char* path);
   

2. 设置重力加速度，尤其注意在机械臂倒装或侧装时需向算法库更新这个值

   /**
    * @brief 设置重力加速度向量在机械臂基坐标系下的描述
    * @param vec[0]: 重力加速度在机械臂基坐标系X轴上的分量
    *        vec[1]: 重力加速度在机械臂基坐标系Y轴上的分量
    *        vec[2]: 重力加速度在机械臂基坐标系Z轴上的分量
    * @return if < 0, 表示设置失败
    */
   ARAL_API_COMMON(1.0) int mdlSetGravityVectorInBase(const Array3d& vec) = 0;
   

3. 设置力矩系数

   /**
    * @brief 设置机械臂各关节输出端的力矩常数, 单位: (Nm/A), 一般从机器人关节中读取
    * @param moduleConstants: 关节输出端力矩常数
    */
   ARAL_API_COMMON(1.0) int mdlSetJointTorqueConstant(const RLJntArray& moduleConstants) = 0;
   

4. 设置工具动力学参数

   /**
    * @brief 设置负载的动力学参数, 参考坐标系为末端法兰坐标系
    * @param inertial: 负载的动力学参数结构体
    * @return < 0, 表示设置失败
    */
   ARAL_API_COMMON(1.0) int mdlSetLoadDynamicParameterInFlange(const RLInertia& inertia) = 0;
   

5. 设置本体动力学参数

   /**
    * @brief 设置机械臂的连杆动力学参数
    * @param real_para: 实际辨识出来的参数，参数格式为: real_para = [参数类型, 辨识的参数]
    *                   1. real_para[0] = 0; size(real_para) = 60 + 1, 输入参数含义为 m, mx, my, mz, ixx, ixy, ixz, iyy, iyz, izz ..., 需要另外设置转子惯量．否则为urdf里面的值．
    *                   2. real_para[0] = 1; size(real_para) = 58 + 1, 对应　aral　动力学标定接口的数据输出格式.
    *                   3. real_para[0] = 2; size(real_para) = 42 + 1, 对应第一版拖动示教导入生产写在底座的数据格式．
    * @return: if < 0, 则设置的参数类型或大小不正确
    */
   ARAL_API_COMMON(1.0) int mdlSetRobotLinkDynamicParameter(const std::vector<double>& real_para) = 0;
   

6. 设置摩擦力参数，当前电流拖动仅支持的摩擦力类型为速度-温度-负载模型，即FrictionParam.type = FrictionType::VTL_Model。其中，type、Fs、Fc和Fv1必须设置到ARAL，否则会报错，其他参数可不设置

   /**
    * @brief 设置关节摩擦参数, 该参数一般通过读取关节驱动中的参数而获得
    * @param friPara: 摩擦模型参数结构体, 长度为机器人自由度(一般是6), 顺序为[J1,J2,J3,J4,J5,J6]
    *                 类型为 VTL_Model 速度-温度-载荷模型
    *                 具体参数及顺序为[Fs, Fc, Vs, Miu, Fv0, Fv1, Fv2, Fv3, Ft1, Ft2, Ft3, c1, c2]
    * @return < 0, 表示设置失败
    */
   ARAL_API_COMMON(1.0) int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara) = 0;
   

7. 设置控制器模式，电流拖动需设置type为CART_IMP_FREE_DRIVE

   /**
    * @brief 设置控制类型
    * @param type:主要有三种类型: 1）位置控制; 2）导纳控制; 3）阻抗控制; 4）零力控制
    *        主类型下面又分一些子类型, 如导纳控制目前支持 0:拖动示教; 1:轨迹跟踪(柔顺); 2:恒力跟踪等功能
    *        子类型无需用户显式指定, 而是根据用户设置的不同参数调整算法控制器结构来实现对应的功能
    * @return 返回设置状态
    */
   ARAL_API_COMMON(1.0) int mcSetControlType(const ControlMode& type) = 0;
   

8. 初始化机械臂状态参数

   /**
    * @brief 初始化规划器状态(根据机械臂的实际状态更新规划起始点), 遇到以下情形需要调用本函数:
    * 1) 第一次创建规划器;
    * 2) 规划返回错误, 错误处理(一般调用急停函数)完成后重新初始化;
    * 3) 切换控制器(机械臂的运动由另外规划器规划)后重新初始化(可以初始化成 IDEL 和 PAUSED 两种状态)
    * @param q: 机械臂规划起始关节角
    * @param qd: 机械臂规划起始关节速度
    * @param qdd: 机械臂规划起始关节加速度
    * @param ToolWorkpiece: 当前运动对应的工具和工件信息
    * @param status: 将规划器初始化成何种状态(目前只支持 IDEL 和 PAUSED(只支持将规划器当前状态初始化成暂停状态) 两种)
    * @return if < 0, 则表示设置失败
    */
   ARAL_API_COMMON(1.0) int rsInitiateRobotState(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const ToolWorkpiece& tool_workpiece, const PlannerStatus& status) = 0;
   

9. 初始化控制器参数，由于ControlMode是笛卡尔阻抗拖动，space选择CARTESIAN

   /**
    * @brief 初始化控制器参数(如果只需要用规划相关的功能，在初始化时调用rsInitiateRobotState即可;
    * 如果还需要用控制相关功能，需要同时调用rsInitiateRobotState和mcInitiateControlPara函数)
    * @param space: 控制坐标系类型
    * @param frame: 如果控制在笛卡尔空间进行, 本参数为任务坐标系相对于基坐标系的位姿
    * @return: 返回值 < 0 表示设置失败
    */
   ARAL_API_COMMON(1.0) int mcInitiateControlPara(const DescribeSpace& space, const RLPose& frame) = 0;
   

10. 设置关节阻尼百分比(对应软件的阻尼进度条，拖动唯一可给用户调整参数)，space 选择 JOINT，damp 取值范围[0, 1]，阻尼百分比为100%对应只补偿重力矩

    /**
    * @brief 设置阻抗控制阻尼参数, 参数的数值必须是正数
    * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
    * @param damp: 阻尼，笛卡尔空间单位N*s/m，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
    *             关节空间单位Nm*s/rad，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
    *             阻尼数值越大，相同速度产生的阻力/力矩越大。
    * @return: 返回设置状态
    */
    ARAL_API_COMMON(1.0) int fcSetDamp(const DescribeSpace& space, const double* damp) = 0;
    

11. 调用硬件抽象层接口，设置驱动器速度、电流滤波参数。电流30，速度20

     robot->setCurrentFilterParams(cur_fre);   
     robot->setVelocityFilterParams(vel_fre);
    

2 计算指令力矩输出(实时)

1. 更新机器人运动学状态

   /**
    * @brief 更新机器人关节的实时状态信息。(如果用户不能给出全部信息, 可以先调用 utlKalmanFilterPVA 函数进行估计)
    *        调用这个接口会连带更新机械臂所有连杆在笛卡尔空间的运动学信息, 包括位姿, 速度和加速度等
    * @param q: 关节当前位置
    * @param qd: 关节当前速度
    * @param qdd: 关节当前加速度
    * @param　is_user_acc: 是否采用用户输入的加速度(默认是否)
    * @return if < 0, 表示设置失败
    */
   ARAL_API_COMMON(1.0) int rsUpdateJointPVA(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const bool& is_user_acc = false) = 0;
   

2. 更新机器人动力学状态，其中摩擦力若设置为0，则由ARAL计算；若设置不为0，则须将emalg工程计算的关节摩擦力传入ARAL

   /**
    * @brief 更新机器人关节的实时关节力矩/摩擦和温度信息, 用于碰撞监测或估计外力
    * @param toruqe: 关节实际力矩, 单位Nm
    * @param temperature: 关节温度, 单位为摄氏度(℃)
    * @param fricition: 关节摩擦力，单位为Nm
    * @return if < 0, 表示设置失败
    */
   ARAL_API_COMMON(1.0) int rsUpdateJointCTF(const RLJntArray& toruqe, const RLJntArray& temperature, const RLJntArray& friction) = 0;
   

3. 设置目标力，拖动功能目标力设为0

   /**
    * @brief 设置机械臂在控制坐标系下的目标力/力矩(该目标有两个属性. 如果是动态坐标系或者时变力,则每个控制周期都需要设置)
    * @param space: 显式指明在关节或者笛卡尔进行力控
    * @param frame: 如果是笛卡尔空间力控, 则frame表示任务坐标系相对与base的位姿; 如果是关节控制, 则frame需要传入单位阵, 对应的各个元素设置成0
    * @param force: 在控制坐标系下的wrench
    * @return 返回值 < 0 表示设置失败
    */
   ARAL_API_COMMON(1.0) int fcSetGoalForce(const DescribeSpace& space, const RLPose& frame, const RLWrench& force) = 0;
   

4. 设置参考轨迹，拖动功能参考轨迹为空

   /**
    * @brief 设置参考轨迹1(用户设置参考位置时，必须同时设置参考速度和参考加速度;否则可以都设置为空)
    * @param type: 参考轨迹描述坐标系,具体可参考 DescribeSpace 定义
    * @param frame: 如果type不等于JOINT, 则frame表示参考轨迹坐标系相对基坐标系的相对位姿关系
    * @param positions: 参考轨迹的位置信息, 如没有, 则置空
    * @param velocities: 参考轨迹的速度信息, 如没有, 则置空
    * @param accelerations: 参考轨迹的加速度信息, 如没有, 则置空
    * @return if < 0, 则表示设置失败
    */
   ARAL_API_COMMON(1.0) int rsSetReferenceTrajectory(const DescribeSpace& type, const RLPose& frame, const double* positions,const double* velocities, const double* accelerations) = 0;
   

5. 计算控制输出指令力矩

   /**
    * @brief 计算关节的控制指令参数.(调用该函数时, 如果算法出错, 则返回数据不可用)
    * @param res: 返回值, 包括关节位置、速度、加速度和力矩
    * @param ik_eps: 逆解迭代精度参数
    * @return return: 0 - 计算正确
    *            -60003 - 原因: 运动速度超限 解决方法: 如果是运动控制调整规划轨迹设置的最大速度, 如果是力控调整力控参数
    *            -60009 - 原因: 关节位置超限 解决方法: 如果是运动控制调整规划轨迹设置的目标路点, 如果是力控调整力控参数
    *            -60013 - 原因: 参考轨迹非法 解决方法: 调整为符合相应功能要求的参考轨迹类型
    *            -60015 - 原因: 机器人奇异  解决方法: 轴动到非奇异区域
    *            -30027 - 原因: 没有和参考角在同一关节子空间的逆解 解决方法: 未实现在子空间内搜索逆解, 建议更改选解条件, RobotConfiguration::NONE
    *            -30006 - 原因: IK 计算失败 解决方法: 机械臂在当前位置没有解, 且没有使能迭代解, 建议使能迭代解
    */
   ARAL_API_COMMON(1.0) int rsCalJointCommand(JointCommand& result, const Array2d& ik_eps = {ARAL_IK_ITER_POS_EPS, ARAL_IK_ITER_ORI_EPS}) = 0;
   

3 拖动测试步骤和常见问题

按照算法接口调用流程开发电流拖动功能后，在进行实机测试时的一般性步骤:

1. 确认机器人状态

   • [ ] 机器人是否正常上电

   • [ ] 各状态量返回是否正常，包括：关节位置、速度、加速度(Canlite协议)、电流、温度等

2. 确认rsCalJointCommand指令力矩 (重要！！)

   • [ ] 对于初次测试电流拖动的机器人，为安全起见在位置模式下运行轨迹，采集关节实际电流与指令电流(ARAL接口计算出来的是力矩，注意量纲一致性)，对比两者趋势是否一致，差值是否在模型误差范围内（模型误差具体值参考碰撞检测阈值）

3. 开启自由拖动

   • [ ] 拖动末端或关节至尽量不同的位姿，测试机器人的正常拖动状态

   • [ ] 拖动关节至极限位置(软件若不设定则为URDF默认值)，关节不可越过极限位置并有反弹现象，松手后回弹至相同位置

   • [ ] 快速(大概>1.8rad/s)拖动机器人，感觉到速度越快阻尼越大的手感

在开启自由拖动后，可能会遇到一些问题:

• 拖动关节或末端上下手感轻重不一致

  可能原因: 大概率是力矩系数不准，如果是向上轻力矩系数偏小，反之力矩系数偏大

  解决方法: 可以尝试在原有力矩系数基础上调整，调用mdlSetJointTorqueConstant接口重新设置到算法库，重新测试

  ​ 如果证实是力矩系数的问题，需要确认力矩系数来源、辨识过程等

• 开启拖动后飘动

  可能原因:

  • 本体或工具动力学参数、重力加速度、力矩系数等不准
  • 摩擦力系数上下限偏大或摩擦参数不准

  解决方法:

  1. 将阻尼进度条设置为100%(此时只补重力)，如果仍然飘检查确认第一条可能原因中的参数；如果不飘，进行第2步
  2. 将摩擦力系数上下限改小，如果仍然飘需要再检查确认摩擦参数

  摩擦系数上下限选择的依据:

  • 理论依据----根据每个型号模块的摩擦建模精度选择摩擦力补偿的系数
  • 实机测试----摩擦系数取较大值时拖动如果飘，阻尼百分比调至99%，如果此时仍飘减小摩擦系数，反复调整找出上界

                    摩擦系数取较小值时拖动如果沉，阻尼百分比调至0%，如果此时仍沉增大摩擦系数，反复调整找出下界
    

  不飘的衡量标准是拖动阻尼百分比在[0-100%]范围都不会飘